/* Author: YangSaqing
 * Last edit:2019-03-07
 *
 */
#include "coord.h"
#include <math.h>
//CGCS-2000
static const double a=6378137.00;//semi-major axis
static const double b=6356752.3142;
static const double f=1/298.257222101;//flattening
static const double e2=0.006694380022901;

CoordLBH::CoordLBH(const double &longitude, const double &latitude, const double &height)
{
    set(longitude,latitude,height);
}
void CoordLBH::set(const double &longitude, const double &latitude, const double &height)
{
    this->longitude = longitude;
    this->latitude = latitude;
    this->height = height;
}
CoordXYZ CoordLBH::toXYZ() const
{
    CoordXYZ coordXYZ;
    double N = a/sqrt(1-e2*sin(latitude)*sin(latitude));
    coordXYZ['x'] = (N+height)*cos(latitude)*cos(longitude);
    coordXYZ['y'] = (N+height)*cos(latitude)*sin(longitude);
    coordXYZ['z'] = (N*(1-e2)+height)*sin(latitude);
    return coordXYZ;
}
CoordOXYZ CoordLBH::toOXYZ(const CoordLBH &localLBH) const
{
    CoordXYZ localXYZ = localLBH.toXYZ();
    CoordXYZ targetXYZ = this->toXYZ();
    CoordOXYZ targetOXYZ;
    targetOXYZ['x'] = cos(localLBH['b'])*(targetXYZ['z']-localXYZ['z'])-sin(localLBH['b'])*cos(localLBH['l'])*(targetXYZ['x']-localXYZ['x'])-sin(localLBH['b'])*sin(localLBH['l'])*(targetXYZ['y']-localXYZ['y']);
    targetOXYZ['y'] = cos(localLBH['l'])*(targetXYZ['y']-localXYZ['y'])-sin(localLBH['l'])*(targetXYZ['x']-localXYZ['x']);
    targetOXYZ['z'] = cos(localLBH['b'])*cos(localLBH['l'])*(targetXYZ['x']-localXYZ['x'])+cos(localLBH['b'])*sin(localLBH['l'])*(targetXYZ['y']-localXYZ['y'])+sin(localLBH['b'])*(targetXYZ['z']-localXYZ['z']);
    return targetOXYZ;
}
CoordRAE CoordLBH::toRAE(const CoordLBH &local) const
{
    return this->toOXYZ(local).toRAE();
}

CoordXYZ::CoordXYZ(const double &X, const double &Y, const double &Z)
{
    set(X,Y,Z);
}
void CoordXYZ::set(const double &X, const double &Y, const double &Z)
{
    this->X = X;
    this->Y = Y;
    this->Z = Z;
}

CoordLBH CoordXYZ::toLBH() const
{
    CoordLBH coordLBH;
    double cir; //在迭代计算中，作为结束的判断标志
    cir=1.57e-10;
    double B_copy; //用来储存上一个B值

    double N = a;
    double H = sqrt(X*X+Y*Y+Z*Z)-sqrt(a*b);
    double B = atan2(Z,sqrt(X*X+Y*Y)*(1-e2*N/(N+H)));
    double L;
    L=atan2(Y,X);   //由于L值不需迭代，可以提前算出

    int i = 0;
    do
    {
        //此处防止死循环，由于计算精度原因，当sqrt(x²+y²+z²）很小（即接近地心）时，会出现迭代过程无法收敛的异常情况，因此在此处 限制迭代次数上限1000次。
        if(++i>1000)
            break;
        B_copy=B;
        N=a/sqrt(1-e2*sin(B)*sin(B));
        H=sqrt(X*X+Y*Y)/cos(B)-N;
        B=atan2(Z,sqrt(X*X+Y*Y)*(1-e2*N/(N+H)));
    }
    while(fabs(B-B_copy)>cir);
    coordLBH['l'] = L;
    coordLBH['b'] = B;
    coordLBH['h'] = H;
    return coordLBH;
}
CoordOXYZ CoordXYZ::toOXYZ(const CoordLBH &local) const
{
    return this->toLBH().toOXYZ(local);
}
CoordRAE CoordXYZ::toRAE(const CoordLBH &local) const
{
    return this->toOXYZ(local).toRAE();
}

CoordRAE::CoordRAE(const double &R, const double &A, const double &E)
{
    set(R,A,E);
}
void CoordRAE::set(const double &R, const double &A, const double &E)
{
    this->R = R;
    this->A = A;
    this->E = E;
}
CoordOXYZ CoordRAE::toOXYZ() const
{
    CoordOXYZ coordOXYZ;
    coordOXYZ['x'] = R*cos(E)*cos(A);
    coordOXYZ['y'] = R*cos(E)*sin(A);
    coordOXYZ['z'] = R*sin(E);
    return coordOXYZ;
}
CoordXYZ CoordRAE::toXYZ(const CoordLBH &local) const
{
    return this->toOXYZ().toXYZ(local);
}
CoordLBH CoordRAE::toLBH(const CoordLBH &local) const
{
    return this->toXYZ(local).toLBH();
}

CoordOXYZ::CoordOXYZ(const double &oX, const double &oY, const double &oZ)
{
    set(oX,oY,oZ);
}
void CoordOXYZ::set(const double &oX, const double &oY, const double &oZ)
{
    this->oX = oX;
    this->oY = oY;
    this->oZ = oZ;
}
CoordRAE CoordOXYZ::toRAE() const
{
    CoordRAE coordRAE;
    coordRAE['r'] = sqrt(oX*oX+oY*oY+oZ*oZ);
    coordRAE['a'] = atan2(oY,oX);
    coordRAE['e'] = atan2(oZ,sqrt(pow(oX,2)+pow(oY,2)));
    return coordRAE;
}
CoordXYZ CoordOXYZ::toXYZ(const CoordLBH &local) const
{
    double aa[3][3]={{0-sin(local['b'])*cos(local['l']),0-sin(local['l']),cos(local['b'])*cos(local['l'])},
                       {0-sin(local['b'])*sin(local['l']),cos(local['l']),cos(local['b'])*sin(local['l'])},
                       {cos(local['b']),0,sin(local['b'])}};
    double c[3];
    for(int i=0;i<3;i++)
    {
        c[i]=aa[i][0]*oX+aa[i][1]*oY+aa[i][2]*oZ;
    }
    CoordXYZ localXYZ = local.toXYZ();
    CoordXYZ target;
    target['x'] = localXYZ['x'] + c[0];
    target['y'] = localXYZ['y'] + c[1];
    target['z'] = localXYZ['z'] + c[2];
    return target;
}
CoordLBH CoordOXYZ::toLBH(const CoordLBH &local) const
{
    return this->toXYZ(local).toLBH();
}

std::istream& operator >>(std::istream& istr, CoordLBH& coord)
{
    istr >>coord.longitude>>coord.latitude>>coord.height;
    return istr;
}

std::istream& operator >>(std::istream& istr, CoordXYZ& coord)
{
    istr>>coord.X>>coord.Y>>coord.Z;
    return istr;
}

std::istream& operator >>(std::istream& istr, CoordRAE& coord)
{
    istr>>coord.R>>coord.A>>coord.E;
    return istr;
}
std::istream& operator >>(std::istream& istr, CoordOXYZ& coord)
{
    istr>>coord.oX>>coord.oY>>coord.oZ;
    return istr;
}

std::ostream& operator <<(std::ostream& ostr, const CoordLBH& coord)
{
    ostr<<"("<<coord.longitude<<", "<<coord.latitude<<", "<<coord.height<<")";
    return ostr;
}

std::ostream& operator <<(std::ostream& ostr, const CoordXYZ& coord)
{
    ostr<<"("<<coord.X<<", "<<coord.Y<<", "<<coord.Z<<")";
    return ostr;
}

std::ostream& operator <<(std::ostream& ostr, const CoordRAE& coord)
{
    ostr<<"("<<coord.R<<", "<<coord.A<<", "<<coord.E<<")";
    return ostr;
}

std::ostream& operator <<(std::ostream& ostr, const CoordOXYZ& coord)
{
    ostr<<"("<<coord.oX<<", "<<coord.oY<<", "<<coord.oZ<<")";
    return ostr;
}
double& CoordLBH::operator [](const char arg)
{
    if(arg=='l')
        return longitude;
    if(arg=='b')
        return latitude;
    if(arg=='h')
        return height;
    return longitude;
}

double CoordLBH::operator [](const char arg) const
{
    if(arg=='l')
        return longitude;
    if(arg=='b')
        return latitude;
    if(arg=='h')
        return height;
    return longitude;
}
double& CoordXYZ::operator [](const char arg)
{
    if(arg=='x')
        return X;
    if(arg=='y')
        return Y;
    if(arg=='z')
        return Z;
    return X;

}

double CoordXYZ::operator [](const char arg) const
{
    if(arg=='x')
        return X;
    if(arg=='y')
        return Y;
    if(arg=='z')
        return Z;
    return X;

}

double& CoordRAE::operator [](const char arg)
{
    if(arg=='r')
        return R;
    if(arg=='a')
        return A;
    if(arg=='e')
        return E;
    return R;
}

double CoordRAE::operator [](const char arg) const
{
    if(arg=='r')
        return R;
    if(arg=='a')
        return A;
    if(arg=='e')
        return E;
    return R;
}

double& CoordOXYZ::operator [](const char arg)
{
    if(arg=='x')
        return oX;
    if(arg=='y')
        return oY;
    if(arg=='z')
        return oZ;
    return oX;
}

double CoordOXYZ::operator [](const char arg) const
{
    if(arg=='x')
        return oX;
    if(arg=='y')
        return oY;
    if(arg=='z')
        return oZ;
    return oX;
}
